import svar
import cv2
import os
import numpy as np

orbslam3=svar.load('svar_orbslam3')

settings={"sensor":"monocular"}
intrinsic=[752,480,458.654,457.296,367.215,248.375,-0.28340811,0.07395907,0.00019359,1.76187114e-05]
settings["camera"]={"type":"PinHole","parameters":intrinsic}
settings["vocabulary"]='/data/zhaoyong/Program/Thirdparty/ORB_SLAM3/Vocabulary/ORBvoc.txt'
sys=None

#orbslam3.SE3([])
#exit(0)
folder='/data/zhaoyong/Dataset/Euroc/mav0/cam0'

datacsv=open(os.path.join(folder,"data.csv"))
datacsv.readline()
times=datacsv.readlines()

for line in times:
 tm_path=line.split(',')
 tm=float(tm_path[0])
 path=os.path.join(folder,'data',tm_path[1].replace('\n',''))
 print("reading",path)
 img=cv2.imread(path)
 if img is None:
   continue
 mat=orbslam3.Mat(img.data)
 if sys is None:
   sys=orbslam3.System(settings)
 ret=sys.TrackMonocular(mat,tm/1e9,[],'')
 print(ret)
